#include <asio_driver/device/rc_sbus.hpp>
#include <rclcpp/rclcpp.hpp>

#include <general_interface/msg/control_signal.hpp>
#include <general_interface/srv/mode_selection.hpp>

class RemoteNode : public rclcpp::Node
{
  private:
    AD::AsioContext::SharePtr context;
    std::shared_ptr<AD::RC_Sbus> sbus;
    rclcpp::Publisher<general_interface::msg::ControlSignal>::SharedPtr pub_control_signal;
    rclcpp::Client<general_interface::srv::ModeSelection>::SharedPtr srv_client_mode_switch;
    rclcpp::TimerBase::SharedPtr timer;
    int channelX;
    int channelY;
    double scaleX;
    double scaleY;
    AD::RC_Sbus::Frame::ModeChannelValue last_mode_value = AD::RC_Sbus::Frame::ModeChannelValue::ErrorNotMode;

  public:
    RemoteNode() : Node("remote_node")
    {
        context = AD::AsioContext::CreatAsioContext();
        sbus = std::make_shared<AD::RC_Sbus>(context);
        if (sbus->Open("/dev/ttyS7"))
        {
            RCLCPP_INFO(get_logger(), "SBUS device opened successfully");
        }
        else
        {
            RCLCPP_ERROR(get_logger(), "Failed to open SBUS device");
            throw std::runtime_error("Failed to open SBUS device");
        }

        pub_control_signal = this->create_publisher<general_interface::msg::ControlSignal>("control_signal", 1);
        srv_client_mode_switch = this->create_client<general_interface::srv::ModeSelection>("auto_mode");
        timer = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RemoteNode::OnTick, this));

        while (srv_client_mode_switch->wait_for_service(std::chrono::seconds(1)))
        {
            if (!rclcpp::ok())
            {
                RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
            }
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available, waiting again...");
        }

        channelX = get_parameter_or("ChannelX", 0);
        channelY = get_parameter_or("ChannelY", 1);
        scaleX = get_parameter_or("ScaleX", 1.0);
        scaleY = get_parameter_or("ScaleY", 1.0);
    }

  private:
    void OnTick()
    {
        general_interface::msg::ControlSignal msg;
        msg.timestamp = this->now();
        context->SpinOnce();
        auto frame = sbus->ReadFrame();
        if (frame.IsDisconnect())
        {
            RCLCPP_WARN_THROTTLE(get_logger(), *this->get_clock(), std::chrono::seconds(5).count(),
                                 "SBUS device disconnected");
            msg.rotate_control = 0;
            msg.forward_control = 0;
            pub_control_signal->publish(msg);
            return;
        }
        msg.rotate_control = frame.Channel(channelX, -1, 1) * scaleX;
        msg.forward_control = frame.Channel(channelX, -1, 1) * scaleY;
        pub_control_signal->publish(msg);
        if (frame.ModeChannel(4) != last_mode_value)
        {
            if (frame.ModeChannel(4) == AD::RC_Sbus::Frame::ModeChannelValue::Up)
            {
                SwitchAutoMode(true);
            }
            else if (frame.ModeChannel(4) == AD::RC_Sbus::Frame::ModeChannelValue::Down)
            {
                SwitchAutoMode(false);
            }
        }
        RCLCPP_INFO_THROTTLE(get_logger(), *this->get_clock(), std::chrono::seconds(5).count(),
                             "SBUS input: angular=%f, velocity=%f", msg.rotate_control, msg.forward_control);
    }

    void SwitchAutoMode(bool enable)
    {
        auto request = std::make_shared<general_interface::srv::ModeSelection::Request>();
        request->mode = enable ? general_interface::srv::ModeSelection::Request::AUTO
                               : general_interface::srv::ModeSelection::Request::MANUAL;
        auto future = srv_client_mode_switch->async_send_request(request);
        if (rclcpp::spin_until_future_complete(this->shared_from_this(), future, std::chrono::milliseconds(100)) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            auto response = future.get();
            RCLCPP_INFO(get_logger(), "Switch to auto mode: %d", response->success);
        }
        else
        {
            RCLCPP_ERROR(get_logger(), "Failed to switch mode");
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RemoteNode>());
    rclcpp::shutdown();
    return 0;
}